import differential as p1
import numpy as np
import matplotlib.pyplot as mp
from math import sqrt

d = 3.14
step = 0.001
# tige de longueur  10cm
N=3500
l=0.1
g=9.81

## pendule a deux maillons ##

# longueur de le tige 10cm masse de 1g
m = 1

# Y=[y1, y2, y1', y2'] F(Y,t)=[y1', y2', y1'', y2''] ou y1'' et y2'' verifient les equations du pendule double

def modelisation_pendule_double(theta10, theta20):
	A0 = np.array([theta10, theta20, 0., 0.])
	T0 = 0

	pendule_double_expe = lambda y, t: np.array([y[2], y[3], ((-3*g*m*np.sin(y[0])-m*g*np.sin(y[0]-2*y[1])-2*m*l*np.sin(y[0]-y[1])*(y[3]**2+(y[2]**2)*np.cos(y[0]-y[1]))) / (l*m*(3-np.cos(2*(y[0]-y[1]))))) , ((2*np.sin(y[0]-y[1])*((y[2]**2)*2*m*l+2*m*g*np.cos(y[0])+(y[3]**2)*l*m*np.cos(y[0]-y[1]))) / (l*m*(3-np.cos(2*(y[0]-y[1])))))])

	V = p1.meth_N_step(A0, T0, N, step, pendule_double_expe, p1.step_rk4)
	T_expe_1 = []
	T_expe_2 = []
	for i in range (np.shape(V)[0]):
		T_expe_1 = T_expe_1 + [V[i][0]]
	for i in range (0, np.shape(V)[0]):
		T_expe_2 = T_expe_2 + [V[i][1]]

	plt1,=mp.plot(T_expe_1)
	plt2,=mp.plot(T_expe_2)
	mp.legend([plt1, plt2],["theta1(t) avec theta1(0)=PI/2","theta2(t) avec theta2(0)=0"])
	mp.show()


def trajectoire_pendule_double(theta10, theta20):
	A0=np.array([theta10, theta20, 0., 0.])
	T0=np.array([0.])

	pendule_double_expe = lambda y, t: np.array([y[2], y[3], ((-3*g*m*np.sin(y[0])-m*g*np.sin(y[0]-2*y[1])-2*m*l*np.sin(y[0]-y[1])*(y[3]**2+(y[2]**2)*np.cos(y[0]-y[1]))) / (l*m*(3-np.cos(2*(y[0]-y[1]))))) , ((2*np.sin(y[0]-y[1])*((y[2]**2)*2*m*l+2*m*g*np.cos(y[0])+(y[3]**2)*l*m*np.cos(y[0]-y[1]))) / (l*m*(3-np.cos(2*(y[0]-y[1])))))])


	V = p1.meth_N_step(A0, T0, N, step, pendule_double_expe, p1.step_rk4)
	T_expe_1 = []
	T_expe_2 = []
	for i in range (np.shape(V)[0]):
		T_expe_1 = T_expe_1 + [V[i][0]]
	for i in range (np.shape(V)[0]):
		T_expe_2 = T_expe_2 + [V[i][1]]
			
	a2=[]
	b2=[]
	for i in range (np.shape(V)[0]):
		b2 = b2 + [- l*np.cos(T_expe_1[i]) - l*np.cos(T_expe_2[i]+T_expe_1[i])]
		a2 = a2 + [l*np.sin(T_expe_1[i]) + l*np.sin(T_expe_2[i]+T_expe_1[i])]
	return (a2, b2)


def plot_pendule_double ():
	(x1, y1) = trajectoire_pendule_double(3.14/2., 0.)
	plt1, = mp.plot (x1, y1)
	(x2, y2) = trajectoire_pendule_double(3.14/2. + 0.1, 0.)
	plt2, = mp.plot (x2, y2)
	mp.legend([plt1,plt2],["theta1(0)=PI/2, theta2(0)=0","theta1(0)=PI/2+0.1, theta2(0)=0"])
	mp.show()



def plot_difference ():

	(x1, y1) = trajectoire_pendule_double(3.14/2., 0.)
	(x2, y2) = trajectoire_pendule_double(3.14/2. + 0.1, 0.)
	(x3, y3) = trajectoire_pendule_double(3.14/2. + 0.01, 0.)
	(x4, y4) = trajectoire_pendule_double(3.14/2. + 0.001, 0.)
	x = []
	y = []
	z = []
	for i in range (N):
		x = x + [np.sqrt((x1[i]-x2[i])**2+(y1[i]-y2[i])**2)]
	for i in range (N):
		y = y + [10*np.sqrt((x1[i]-x3[i])**2+(y1[i]-y3[i])**2)]
	for i in range (N):
		z = z + [100*np.sqrt((x1[i]-x4[i])**2+(y1[i]-y4[i])**2)]

	plt1,=mp.plot(x)
	plt2,=mp.plot(y)
	plt3,=mp.plot(z)
	mp.legend([plt1,plt2,plt3],[
		"distance entre 2 trajectoires pour un diff de CI de 0.1rad",
		"10 * distance entre 2 trajectoires pour un diff de CI de 0.01rad",
		"100 * distance entre 2 trajectoires pour un diff de CI de 0.001rad"])
	mp.show()


plot_pendule_double ()
modelisation_pendule_double(3.14/2., 0.)
plot_difference()